#ifndef ALGO_PRM_PRIVATE_H
#define ALGO_PRM_PRIVATE_H

#include "algohub_common_type.h"
#include <vector>
#include <map>
#include <memory>
#include <random>
#include <string>

// Forward declaration for a third-party KD-tree library if used
// For now, we will implement a brute-force nearest neighbor search.

namespace algo 
{
    // Represents a node in the roadmap graph
    struct RoadmapNode {
        int id;
        algobase::CoordZ position;
        std::vector<std::pair<int, double>> neighbors;

        RoadmapNode(int id = -1, const algobase::CoordZ& pos = {}) : id(id), position(pos) {}
    };

    // Main private implementation class
    class AlgoPRMPrivate 
    {
    public:
        AlgoPRMPrivate();
        ~AlgoPRMPrivate();

        // Public interface implementations
        int buildRoadmap(const algobase::TaskConfig& taskConfig, const std::vector<std::vector<double>>& terrain);
        int findPaths(const algobase::TaskConfig& taskConfig, algobase::Paths& paths);
        int saveRoadmap(const std::string& filepath) const;
        int loadRoadmap(const std::string& filepath);

    private:
        // Offline phase methods
        std::vector<algobase::CoordZ> sampleNodes();
        void connectNodes();
        void checkConnectivity() const;
        
        // Online phase methods
        int connectPointToGraph(const algobase::Coord& point, const std::string& type);
        void disconnectPointFromGraph(int node_id);
        std::vector<int> findRepresentativeNodeIdsForZone(const algobase::Polygon& polygon);
        std::vector<algobase::Path> findKShortestPaths(int start_id, int end_id, int K);
        algobase::Paths combinePaths(const std::vector<std::vector<algobase::Path>>& all_segment_paths);
        
        // Common helpers
        bool isPointValid(const algobase::Coord& p) const;
        bool isEdgeValid(const algobase::CoordZ& p1, const algobase::CoordZ& p2) const;
        double getDepth(const algobase::Coord& coord) const;
        bool isInside(const algobase::Coord& point, const algobase::Polygon& polygon) const;
        double distance(const algobase::CoordZ& p1, const algobase::CoordZ& p2) const;
        std::vector<int> findKNearestNeighbors(const algobase::CoordZ& pos, int K);
        algobase::Path reconstructGraphPath(const std::vector<int>& node_ids);

        // Data members
        std::map<int, RoadmapNode> graph_;
        int next_node_id_ = 0;
        const algobase::TaskConfig* current_task_config_ = nullptr;
        std::vector<std::vector<double>> terrain_data_;
        std::unique_ptr<std::mt19937> random_generator_;
    };
}

#endif // ALGO_PRM_PRIVATE_H